#include "scene.h"
#include "odegeomwrap.h"
#include "transformable.h"
#include "transform.h"
//#include "utility.h"
#include "matrixcom.h"
#include <osg/Geode>
#include <osg/MatrixTransform>
#include <osg/NodeVisitor>
#include <osg/Material>
#include <osg/PolygonMode>
#include <osg/PolygonOffset>
#include <osg/ShapeDrawable>
#include <osg/StateSet>
#include <osg/Version>
#include <osg/CameraNode>
#include <cassert>
#include "datatypedefine.h"

namespace CoreBase
{
	const std::string Transformable::COLLISION_GEODE_ID("__PORT3D_COLLISION_GEOMETRY__");

	class CollectParentPaths : public osg::NodeVisitor
	{
	public:
		CollectParentPaths(osg::Node* haltTraversalAtNode=0)
			: osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_PARENTS)
			, _haltTraversalAtNode(haltTraversalAtNode)
		{
			//this will force the traversal over nodes that have a mask of 0x0
			this->setNodeMaskOverride(0xffffffff);
		}
		virtual void apply(osg::Node& node)
		{
			if (node.getNumParents()==0 || &node==_haltTraversalAtNode)
			{
				_nodePaths.push_back(getNodePath());
			}
			else
			{
				traverse(node);
			}
		}
		osg::Node*        _haltTraversalAtNode;
		osg::NodePathList _nodePaths;
	};

	class TransformableImpl
	{
	public:
		TransformableImpl(Transformable::TransformableNode& node)
			: _geomWrap(new ODEGeomWrap())
			  , _geomGeod(NULL)
			  , _node(&node)
			  , mRenderingGeometry(false)
			  , mRenderProxyNode(false)
		{}
		osg::ref_ptr<ODEGeomWrap> _geomWrap;
		osg::ref_ptr<osg::Geode> _geomGeod;
		osg::ref_ptr<Transformable::TransformableNode> _node;
		bool mRenderingGeometry;
		bool mRenderProxyNode;
	};

	Transformable::Transformable(ActorBase* owneractor,const QString& name)
		: Drawable(owneractor,name)
		, _impl(new TransformableImpl(*new TransformableNode)),_haveInScene(false)
	{
		//_impl->_node->setName(getName());
		ctor();
	}

	Transformable::Transformable(TransformableNode& node, ActorBase* owneractor,const QString& name)
		: Drawable(owneractor,name), _impl(new TransformableImpl(node)),_haveInScene(false)
	{
		ctor();
	}

	void Transformable::ctor()
	{
		setNormalRescaling( true );
		setCollisionCategoryBits(OBJECT_MASK_TRANSFORMABLE);
		setCollisionCollideBits(OBJECT_MASK_ALL);
	}

	Transformable::~Transformable()
	{
		_impl->_geomWrap = NULL;
		delete _impl;
		_impl = NULL;
	}

	osg::Node* Transformable::getOSGNode()
	{
		return _impl->_node.get();
	}

	const osg::Node* Transformable::getOSGNode() const
	{
		return _impl->_node.get();
	}

	bool Transformable::getAbsoluteMatrix(const osg::Node* node, osg::Matrix& wcmatrix)
	{
		if(node != NULL)
		{
			CollectParentPaths cpp(NULL);
			const_cast<osg::Node*>(node)->accept(cpp);
			osg::NodePathList nodePathList = cpp._nodePaths;
			if(!nodePathList.empty())
			{
				osg::NodePath nodePath = nodePathList[0];
				wcmatrix.set(osg::computeLocalToWorld(nodePath));
				return true;
			}
		}
		return false;
	}

	void Transformable::setTransform(const Transform& xform, CoordSysEnum cs)
	{
		osg::Matrix newMat;
		xform.get(newMat);
		if(cs == ABS_CS)
		{
			if (!getOSGNode()->getParents().empty())
			{
				osg::Matrix parentMat;
				getAbsoluteMatrix(getOSGNode()->getParent(0), parentMat);
				osg::Matrix relMat = newMat * osg::Matrix::inverse(parentMat);
				getMatrixNode()->setMatrix(relMat);
			}
			else
			{
				getMatrixNode()->setMatrix(newMat);
			}
		}
		else if(cs == REL_CS)
		{
			getMatrixNode()->setMatrix(newMat);
		}
		prePhysicsStepUpdate();
	}

	void Transformable::getTransform(Transform& xform, CoordSysEnum cs) const
	{
		const TransformableNode* mt = getMatrixNode();
		if(cs == ABS_CS)
		{
			osg::Matrix newMat;
			getAbsoluteMatrix(mt, newMat);
			xform.set( newMat );
		}
		else if(cs == REL_CS)
		{
			xform.set(mt->getMatrix());
		}
	}

	Transformable::TransformableNode* Transformable::getMatrixNode()
	{
		return _impl->_node.get();
	}

	const Transformable::TransformableNode* Transformable::getMatrixNode() const
	{
		return _impl->_node.get();
	}

	const osg::Matrix& Transformable::getMatrix() const
	{
		return _impl->_node->getMatrix();
	}

	void Transformable::setMatrix(const osg::Matrix& mat)
	{
		_impl->_node->setMatrix(mat);
	}

	bool Transformable::addChild(Drawable* child)
	{
		if (Drawable::addChild(child))
		{
			getMatrixNode()->addChild(child->getOSGNode());
			return true;
		}
		else
		{
			return false;
		}
	}

	void Transformable::removeChild(Drawable* child)
	{
		getMatrixNode()->removeChild(child->getOSGNode());
		Drawable::removeChild(child);
	}

	void Transformable::setNormalRescaling(const bool enable)
	{
		osg::StateAttribute::GLModeValue state;
		state = (enable) ? osg::StateAttribute::ON : osg::StateAttribute::OFF;
		if(getOSGNode() != NULL)
		{
			getOSGNode()->getOrCreateStateSet()->setMode(GL_RESCALE_NORMAL, state);
		}
	}

	bool Transformable::getNormalRescaling() const
	{
		if(getOSGNode() == NULL)
		{
			return false;
		}
		osg::StateAttribute::GLModeValue state = getOSGNode()->getStateSet()->getMode(GL_RESCALE_NORMAL);
		return (state & osg::StateAttribute::ON) ? true : false;
	}

	QString Transformable::getCollisionGeomType() const
	{
		return _impl->_geomWrap->getCollisionGeomType();
	}
	
	void Transformable::getCollisionGeomDimensions(std::vector<float>& dimensions)
	{
	   prePhysicsStepUpdate();
	   _impl->_geomWrap->getCollisionGeomDimensions(dimensions);
	 }
	
	void Transformable::setCollisionCategoryBits(unsigned long bits)
	{
	   _impl->_geomWrap->setCollisionCategoryBits(bits);
	   //unsigned int count = GetNumChildren();
	   //for (unsigned int index = 0; index < count; ++index)
	   //{
	   //   Transformable* child = dynamic_cast<Transformable*>(GetChild(index));
	   //   if (child)
	   //   {
	   //      child->SetCollisionCategoryBits(bits);
	   //   }
	   //}
	}
	
	unsigned long Transformable::getCollisionCategoryBits() const
	{
	   return _impl->_geomWrap->getCollisionCategoryBits();
	}
	
	void Transformable::setCollisionCollideBits(unsigned long bits)
	{
	   _impl->_geomWrap->setCollisionCollideBits(bits);
	
	   //unsigned int count = GetNumChildren();
	   //for (unsigned int index = 0; index < count; ++index)
	   //{
	   //   Transformable* child = dynamic_cast<Transformable*>(GetChild(index));
	   //   if (child)
	   //   {
	   //      child->SetCollisionCollideBits(bits);
	   //   }
	   //}
	}
	
	unsigned long Transformable::getCollisionCollideBits() const
	{
	   return _impl->_geomWrap->getCollisionCollideBits();
	}
	
	void Transformable::setCollisionDetection(bool enabled)
	{
	   _impl->_geomWrap->setCollisionDetection(enabled);
	   //unsigned int count = GetNumChildren();
	   //for (unsigned int index = 0; index < count; ++index)
	   //{
	   //   Transformable* child = dynamic_cast<Transformable*>(GetChild(index));
	   //   if (child)
	   //   {
	   //      child->SetCollisionDetection(enabled);
	   //   }
	   //}
	}
	
	bool Transformable::getCollisionDetection() const
	{
	   return _impl->_geomWrap->getCollisionDetection();
	}
	
	dGeomID Transformable::getGeomID() const
	{
	   return _impl->_geomWrap->getGeomID();
	}
	
	void Transformable::setCollisionGeom(dGeomID geom)
	{
	   _impl->_geomWrap->setCollisionGeom(geom);
	   //RenderCollisionGeometry(_impl->mRenderingGeometry);
	   prePhysicsStepUpdate();
	}
	
	void Transformable::setCollisionSphere(float radius)
	{
	   _impl->_geomWrap->setCollisionSphere(radius);
	   //RenderCollisionGeometry(_impl->mRenderingGeometry);
	   _impl->_geomWrap->setCollisionDetection(true);
	   //// Sync-up the transforms on mGeomID
	   prePhysicsStepUpdate();
	}
	
	void Transformable::setCollisionSphere(osg::Node* node)
	{
	   if(node == 0)
	   {
	      node = getOSGNode();
	   }
	   if(node)
	   {
	      osg::Matrix oldMatrix = getMatrixNode()->getMatrix();
	      getMatrixNode()->setMatrix(osg::Matrix::identity());
	      _impl->_geomWrap->setCollisionSphere(node);
	      getMatrixNode()->setMatrix(oldMatrix);
	      //RenderCollisionGeometry(_impl->mRenderingGeometry);
	      _impl->_geomWrap->setCollisionDetection(true);
	      prePhysicsStepUpdate();
	   }
	}
	
	void Transformable::setCollisionBox(float lx, float ly, float lz)
	{
	   _impl->_geomWrap->setCollisionBox(lx, ly, lz);
	   //RenderCollisionGeometry(_impl->mRenderingGeometry);
	   setCollisionDetection(true);
	   prePhysicsStepUpdate();
	}
	
	void Transformable::setCollisionBox(osg::Node* node)
	{
	   if(node == 0)
	   {
	      node = getOSGNode();
	   }
	   if(getSceneParent() && node != 0)
	   {
	      osg::Matrix oldMatrix = getMatrixNode()->getMatrix();
	      getMatrixNode()->setMatrix(osg::Matrix::identity());
	      _impl->_geomWrap->setCollisionBox(node);
	      getMatrixNode()->setMatrix(oldMatrix);
	      //RenderCollisionGeometry(_impl->mRenderingGeometry);
	      setCollisionDetection(true);
	      prePhysicsStepUpdate();
	   }
	}
	
	//void Transformable::SetCollisionCylinder(float radius, float length)
	//{
	//   _impl->_geomWrap->SetCollisionCylinder(radius, length);
	//   RenderCollisionGeometry(_impl->mRenderingGeometry);
	//   SetCollisionDetection(true);
	//   prePhysicsStepUpdate();
	//}
	//
	//void Transformable::SetCollisionCylinder(osg::Node* node)
	//{
	//   if(node == 0)
	//   {
	//      node = this->getOSGNode();
	//   }
	//   if(node)
	//   {
	//      osg::Matrix oldMatrix = getMatrixNode()->getMatrix();
	//      getMatrixNode()->setMatrix(osg::Matrix::identity());
	//      _impl->_geomWrap->SetCollisionCylinder(node);
	//      getMatrixNode()->setMatrix(oldMatrix);
	//      RenderCollisionGeometry(_impl->mRenderingGeometry);
	//      SetCollisionDetection(true);
	//      prePhysicsStepUpdate();
	//   }
	//}
	//
	//void Transformable::SetCollisionCappedCylinder(float radius, float length)
	//{
	//   _impl->_geomWrap->SetCollisionCappedCylinder(radius, length);
	//   RenderCollisionGeometry(_impl->mRenderingGeometry);
	//   SetCollisionDetection(true);
	//   prePhysicsStepUpdate();
	//}
	//
	//void Transformable::SetCollisionCappedCylinder(osg::Node* node)
	//{
	//   if(node == 0)
	//   {
	//      node = this->getOSGNode();
	//   }
	//   if(node)
	//   {
	//      osg::Matrix oldMatrix = getMatrixNode()->getMatrix();
	//      getMatrixNode()->setMatrix(osg::Matrix::identity());
	//      _impl->_geomWrap->SetCollisionCappedCylinder(node);
	//      getMatrixNode()->setMatrix(oldMatrix);
	//      RenderCollisionGeometry(_impl->mRenderingGeometry);
	//      SetCollisionDetection(true);
	//      prePhysicsStepUpdate();
	//   }
	//}
	//
	//void Transformable::SetCollisionRay(float length)
	//{
	//   _impl->_geomWrap->SetCollisionRay(length);
	//   RenderCollisionGeometry(_impl->mRenderingGeometry);
	//   SetCollisionDetection(true);
	//   prePhysicsStepUpdate();
	//}
	
	void Transformable::setCollisionMesh(osg::Node* node)
	{
	   if(node == 0)
	   {
	      node = getOSGNode();
	   }
	   if(node)
	   {
	      osg::Matrix oldMatrix = getMatrixNode()->getMatrix();
	      getMatrixNode()->setMatrix(osg::Matrix::identity());
	      _impl->_geomWrap->setCollisionMesh(node);
	      getMatrixNode()->setMatrix(oldMatrix);
	      //RenderCollisionGeometry(_impl->mRenderingGeometry);
	      setCollisionDetection(true);
	   }
	}
	
	void Transformable::clearCollisionGeometry()
	{
	   _impl->_geomWrap->clearCollisionGeometry();
	   if(_impl->_geomGeod.valid())
	   {
	      _impl->_geomGeod->removeDrawables(0,_impl->_geomGeod->getNumDrawables());
	   }
	}
	
	void Transformable::prePhysicsStepUpdate()
	{
	   if (_impl->_geomWrap->getCollisionDetection() == false) {return;}
	   Transform transform;
	   this->getTransform(transform, Transformable::ABS_CS);
	   _impl->_geomWrap->updateGeomTransform(transform);
	}

	void Transformable::renderCollisionGeometry(bool enable,bool wireFrame)
	{
	   TransformableNode *xform = this->getMatrixNode();
	   if(!xform)
	   {
	      return;
	   }
	   _impl->mRenderingGeometry = enable;
	   if(enable)
	   {
	      if (_impl->_geomGeod.valid())
	      {
	         removeRenderedCollisionGeometry();
	      }
	      _impl->_geomGeod = 
	         _impl->_geomWrap->createRenderedCollisionGeometry(wireFrame);
	      if (_impl->_geomGeod.valid())
	      {
	         _impl->_geomGeod->setName(Transformable::COLLISION_GEODE_ID);
	         xform->addChild(_impl->_geomGeod.get());
	      }
	   }
	   else
	   {
	      this->removeRenderedCollisionGeometry();
	   }
	   unsigned int count = getNumChildren();
	   for (unsigned int index = 0; index < count; ++index)
	   {
	      Transformable* child = dynamic_cast<Transformable*>(getChild(index));
	      if (child)
	      {
	         child->renderCollisionGeometry(enable);
	      }
	   }
	}
	
	bool Transformable::getRenderCollisionGeometry() const
	{
	   return _impl->mRenderingGeometry;
	}

	void Transformable::addedToScene(Scene* scene)
	{
		if(scene)
		{
			if(getSceneParent())
			{
				getSceneParent()->removeDrawable(this);
			}
			Drawable::addedToScene(scene);
			//scene->registerCollidable(this);
			_haveInScene = true;
		}
		else
		{
			//if(getSceneParent())
			//{
			//	getSceneParent()->unRegisterCollidable(this);
			//}
			Drawable::addedToScene(NULL);
			_haveInScene = false;
		}
	}

	void Transformable::removeRenderedCollisionGeometry()
	{
	   if(_impl->_geomGeod.valid())
	   {
	      getMatrixNode()->removeChild(_impl->_geomGeod.get());
	      _impl->_geomGeod = 0;
	   }
	}

	const ODEGeomWrap* Transformable::getGeomWrapper() const
	{
	   return _impl->_geomWrap.get();
	}
	
	ODEGeomWrap* Transformable::getGeomWrapper()
	{
	   return _impl->_geomWrap.get();
	}
}